// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBRSimpleCartesianMoveAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Runtime
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.Arm;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    using ShoNS.Array;

    /// <summary>
    /// A class that listens to linear Cartesian motion control,
    /// does simple motion planning, and then uses joint position
    /// controller to execute the trajectory.
    /// This class is supposed to sit above the level of direct
    /// hardware control which is what KukaLBRControlAgent is
    /// in charge of.
    /// </summary>
    [DataContract]
    public class KukaLBRSimpleCartesianMoveAgent : SimpleCartesianMoveAgent
    {
        /// <summary>
        /// Trace context for this class
        /// </summary>
        private static TraceContext moveAgentContext = TraceContexts.FromString(TraceContexts.Manip, "KUKAMoveAgent");

        /// <summary>
        /// Initializes a new instance of the <see cref="KukaLBRSimpleCartesianMoveAgent" /> class.
        /// </summary>
        /// <param name="name">The name of the agent</param>
        /// <param name="producer">The producer which generates arm status</param>
        /// <param name="trajectoryControlAgent">The trajectory control agent which
        /// is in charge of executing a trajectory</param>
        /// <param name="jointTolerance">Joint move tolerance</param>
        /// <param name="minimumJointHeight">Minimum height to avoid collision</param>
        /// <param name="maximumJointVel">Maximum joint velocity</param>
        /// <param name="cutOffPercentageForIKFiltering">The percentage of the top score for IK filtering</param>
        /// <param name="minimumTrajectoryTime">The minimum time for executing a trajectory</param>
        /// <param name="guaranteedClearance">The guaranteed clearance below the end effector</param>
        /// <param name="minimumElbowHeight">Minimum elbow height from table surface</param>
        /// <param name="trajectoryQualityThreshold">The threshold for trajectory quality</param>
        /// <param name="jointWeightFactors">Joint weight factors</param>
        /// <param name="elbowClearanceCheckList">Elbow clearance checklist</param>
        /// <param name="visualizerAgentName">Name of the visualizer</param>
        public KukaLBRSimpleCartesianMoveAgent(
            string name,
            IProducer<ManipulatorStatusMessage> producer,
            IControllable<StartTrajectoryMessage> trajectoryControlAgent,
            double jointTolerance,
            double minimumJointHeight,
            double maximumJointVel,
            double cutOffPercentageForIKFiltering,
            double minimumTrajectoryTime,
            double guaranteedClearance,
            double minimumElbowHeight,
            double trajectoryQualityThreshold,
            double[] jointWeightFactors,
            bool[] elbowClearanceCheckList,
            string visualizerAgentName = null)
            : base(
            name,
            producer,
            trajectoryControlAgent,
            KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain(),
            KukaLBR4KinematicConfigurations.GetKuka7DOFDefaultIKSeeds(),
            jointTolerance,
            minimumJointHeight,
            maximumJointVel,
            cutOffPercentageForIKFiltering,
            minimumTrajectoryTime,
            guaranteedClearance,
            minimumElbowHeight,
            trajectoryQualityThreshold,
            jointWeightFactors,
            elbowClearanceCheckList,
            visualizerAgentName)
        {
        }

        /// <summary>
        /// Routine to initialize this object
        /// </summary>
        /// <param name="locator">Agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            System.Console.WriteLine("KukaLBRSimpleCartesianMoveAgent is initialized");
            TraceOut.Info(moveAgentContext, "KukaLBRSimpleCartesianMoveAgent is initialized");
        }

        /// <summary>
        /// Inverse kinematics extender for Kuka arm
        /// </summary>
        /// <param name="jointValues">Joint values</param>
        /// <returns>A list of equivalent joint values</returns>
        public override List<DoubleArray> InverseKinematicsExtender(DoubleArray jointValues)
        {
            return KukaLBR4KinematicConfigurations.KukaLBR4EquivalentJointConfigurationGenerator(jointValues);
        }
    }
}
